#include "TriFace.h"
#include "TriEdge.h"
#include "TriVertex.h"
#include "TriMesh.h"

#ifdef _DEBUG
static const Real REL_TOL1 = 1e-11f;
static const Real REL_TOL2 = 1e-12f;
static const Real REL_TOL_SQRT = 1e-6f;
#endif


//////////////////////////////////////////////////////////////////////////////
// TriFace functions
//////////////////////////////////////////////////////////////////////////////

TriFace::~TriFace(void)
{
   if (e1) delete e1;
   if (e2) delete e2;
   if (e3) delete e3;
}

void TriFace::Initialize()
{
   id=-1;
   marked=false;
   degenerate=NONE;
   normal=Vector3(1.0, 0.0, 0.0);

   e1=new TriEdge();
   e2=new TriEdge();
   e3=new TriEdge();
   ResetInternalEdgePointers();

}

TriVertex* TriFace::Vertex1(void) const
{
   return e1->Origin();
}

TriVertex* TriFace::Vertex2(void) const
{
   return e2->Origin();
}

TriVertex* TriFace::Vertex3(void) const
{
   return e3->Origin();
}

bool TriFace::Contains(const TriVertex* v) const
{
   return (e1->Origin() == v || e2->Origin() == v || e3->Origin() == v);
}

void TriFace::computeArea()
{
   const Vector3& p1 = Vertex1()->Position();
   const Vector3& p2 = Vertex2()->Position();
   const Vector3& p3 = Vertex3()->Position();
   Vector3 v = v.crossProd((p2 - p1),(p3 - p1));
   area = 0.5f * v.getLength();
}

Vector3 TriFace::getCentroid() 
{
   const Vector3& p1 = Vertex1()->Position();
   const Vector3& p2 = Vertex2()->Position();
   const Vector3& p3 = Vertex3()->Position();
   return (p1 + p2 + p3)*(1.0f/3.0f) ;
}

TriEdge* TriFace::EdgeP(TriFace* f) const
{
   if(e1->Twin()!=NULL)
   {
      if(e1->Twin()->AdjFace()==f)
      {
         return e1;
      }
   }
   if(e2->Twin()!=NULL)
   {
      if(e2->Twin()->AdjFace()==f)
      {
         return e2;
      }
   }
   if(e3->Twin()!=NULL)
   {
      if(e3->Twin()->AdjFace()==f)
      {
         return e3;
      }
   }
   
#ifdef HE_DEBUG
   cerr << "Error: Bad face passed to TriFace::EdgeP" << endl;
#endif
   return NULL;
}

#ifdef _DEBUG
bool TriFace::VerifyTopology(int pos) const
{
    if(Marked())
    {
        cerr << "TriFace at position " << pos << " is marked" << endl;
    }

    return true;
}

// The tests are formulated so as to fail if any of the numbers are NAN
// Since NAN (compare) x = FALSE for all x and all comparison operators.
bool TriFace::VerifyGeometry(int pos) const
{
    bool result = true;

    // Make sure that the edges that are set compute the same normal
    Vector3 temp1 = Vector3::crossProd(e1->Direction(), e2->Direction());

    temp1.normalize();
    if( !(Vector3::dotProd(temp1, normal) < 1.0 + REL_TOL2 &&
         Vector3::dotProd(temp1, normal) > 1.0 - REL_TOL2)
    ) {
        cout << pos << "\t";    
        //cerr << "TriFace at position " << pos << " does not match with that computed using e1 and e2" << endl;
        result = false;
    }

    temp1 = Vector3::crossProd(e2->Direction(), e3->Direction());
    temp1.normalize();
    if( !(Vector3::dotProd(temp1, normal) < 1.0 + REL_TOL2 &&
         Vector3::dotProd(temp1, normal) > 1.0 - REL_TOL2)
    ) {
        //cerr << "TriFace at position " << pos << " does not match with that computed using e2 and e3" << endl;
        result = false;
    }

    temp1 = Vector3::crossProd(e3->Direction(), e1->Direction());
    temp1.normalize();
    if( !(Vector3::dotProd(temp1, normal) < 1.0 + REL_TOL2 &&
         Vector3::dotProd(temp1, normal) > 1.0 - REL_TOL2)
    ) {
        //cerr << "TriFace at position " << pos << " does not match with that computed using e3 and e1" << endl;
        result = false;
    }

    temp1 = Vector3::crossProd(e1->Origin()->PositionInit() - e3->Origin()->PositionInit(),
            e2->Origin()->PositionInit() - e1->Origin()->PositionInit());
    temp1.normalize( );
    if( !(Vector3::dotProd(temp1, normal) < 1.0 + REL_TOL2 &&
         Vector3::dotProd(temp1, normal) > 1.0 - REL_TOL2)
    ) {
       //cerr << "TriFace at position " << pos << " does not match with that computed using vertices 1" << endl;
        result = false;
    }

    temp1 = Vector3::crossProd(e2->Origin()->PositionInit() - e1->Origin()->PositionInit(),
            e3->Origin()->PositionInit() - e2->Origin()->PositionInit());
    temp1.normalize( );
    if( !(Vector3::dotProd(temp1, normal) < 1.0 + REL_TOL2 &&
         Vector3::dotProd(temp1, normal) > 1.0 - REL_TOL2)
    ) {
        //cerr << "TriFace at position " << pos << " does not match with that computed using vertices 2" << endl;
        result = false;
    }

    temp1 = Vector3::crossProd(e3->Origin()->PositionInit() - e2->Origin()->PositionInit(),
            e1->Origin()->PositionInit() - e3->Origin()->PositionInit());
    temp1.normalize( );
    if( !(Vector3::dotProd(temp1, normal) < 1.0 + REL_TOL2 &&
         Vector3::dotProd(temp1, normal) > 1.0 - REL_TOL2)
    ) {
        //cerr << "TriFace at position " << pos << " does not match with that computed using vertices 3 " << endl;
        result = false;
    }

    // Attempt to make sure that the face normal vector is unit length
    if( !(normal.getSquaredLength() < 1.0 + REL_TOL2 &&
          normal.getSquaredLength() > 1.0 - REL_TOL2)
    ) {
        //cerr << "TriFace at position " << pos << " does not have a unit length normal vector" << endl;
        result = false;
    }

    return result;
}
#endif

void TriFace::ComputeGeometry(void)
{
   //Compute geometry of edges
   e1->ComputeGeometry();
   e2->ComputeGeometry();
   e3->ComputeGeometry();
   
   // Compute face plane (this computes the normal)
   ComputePlaneFromEdges();

   // Compute area
   computeArea();
}

void TriFace::ComputeGeometryTwins(void)
{
   //Share the geometry with the twin for each half edge
   
   //Compute the geometry of edges
   e1->ComputeGeometryTwins();
   e2->ComputeGeometryTwins();
   e3->ComputeGeometryTwins();
   
   // Compute face plane (this computes the normal)
   ComputePlaneFromEdges();

   // Compute area
   computeArea();
}

void TriFace::ComputePlaneFromEdges(void)
{
    // It is important for the Fill_Holes function that this use the directions
    // of e2 and e3 only!
   normal = Vector3::crossProd(e2->Direction(), e3->Direction());
   normal.normalize();
   d = Vector3::dotProd(e1->Origin()->Position(), normal);
}

void TriFace::ComputeNormal(void)
{
   normal = Vector3::crossProd(Vertex2()->Position() - Vertex1()->Position(), Vertex3()->Position() - Vertex1()->Position());
   normal.normalize();
}

Vector3 TriFace::GetBarycentricCoords(const Vector3 &p)
{
   Vector3 da, db, dc;
   Real wa, wb, wc;
   Vector3 a,b,c;
   a = Vertex1()->Position(); b = Vertex2()->Position(); c = Vertex3()->Position();

   Vector3 nout = Vector3::crossProd(b-a, c-a);
   da = a - p, db = b - p, dc = c - p;
   wa = Vector3::dotProd(Vector3::crossProd(db, dc), nout);
   wb = Vector3::dotProd(Vector3::crossProd(dc, da), nout);
   wc = Vector3::dotProd(Vector3::crossProd(da, db), nout);

   //Compute barycentric coordinates
   Real area2 = Vector3::dotProd(Vector3::crossProd(b-a,c-a), nout);
   wa /= area2, wb /= area2, wc /= area2;

   return Vector3(wa, wb, wc);
}

bool TriFace::IsPointInsideFace(const Vector3 &p)
{
   Vector3 b = GetBarycentricCoords(p);

   if(b[0] < 0 || b[1] < 0 || b[2] < 0) return false;
   if(b[0] + b[1] + b[2] > 1.00001 || b[0] + b[1] + b[2] < 0.99999)
      return false;
   return true;
}

void TriFace::GetPointProj(Vector3 &p, Vector3& proj, float &d)
{
    Vector3 normal = Vector3::crossProd(Vertex2()->Position() - Vertex1()->Position(), Vertex3()->Position() - Vertex1()->Position());

    d = Vector3::dotProd(normal, p - Vertex1()->Position());
    proj = p - normal*d;
}


void TriFace::ResetInternalEdgePointers(void)
{
   e1->SetNext(e2);
   e2->SetNext(e3);
   e3->SetNext(e1);
   e1->SetAdjFace(this);
   e2->SetAdjFace(this);
   e3->SetAdjFace(this);
}

bool TriFace::IsGrasped(void) const
{
    return (
        Vertex1()->IsGrasped() &&
        Vertex2()->IsGrasped() &&
        Vertex3()->IsGrasped()
        );
}

void TriFace::Draw(TriFaceRender *properties) const
{
   //TriFace Normal
   if(properties->faceNormal)
   {
      if(properties->invertNormal)
      {
         glNormal3f(-normal[0], -normal[1], -normal[2]);
      }
      else
      {
         glNormal3f(normal[0], normal[1], normal[2]);
      }
   }

   //Solid Color
   GLfloat ffacecolor[4];
   GLuint uifacecolor[4];
   if(properties->faceColors){
      ffacecolor[0] = this->color[0];
      ffacecolor[1] = this->color[1];
      ffacecolor[2] = this->color[2];
      ffacecolor[3] = 1.0;

      uifacecolor[0] = this->color[0];
      uifacecolor[1] = this->color[1];
      uifacecolor[2] = this->color[2];
      uifacecolor[3] = 255;
   }

   if(properties->solidColor!=NULL)
   {
      if(properties->faceColors)
         glColor4uiv(uifacecolor);
      else
         glColor4uiv(properties->solidColor);
   }
   
   //Smooth Color
   if(properties->smoothColor!=NULL)
   {
      if (properties->invertNormal && properties->smoothColorInvert!=NULL)
      {
         if(properties->faceColors){
            glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, ffacecolor);
            glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, ffacecolor);
            glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, properties->shininess);
            glColor4fv(ffacecolor);
         }
         else{
            glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, properties->smoothColorInvert);
            glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, properties->smoothColorInvert);
            glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, properties->shininess);
            glColor4fv(properties->smoothColorInvert);
         }    
      }
      else
      {
         if(properties->faceColors){
            glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, ffacecolor);
            glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, ffacecolor);
            glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, properties->shininess);
            glColor4fv(ffacecolor);
         }
         else{
            glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, properties->smoothColor);
            glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, properties->smoothColor);
            glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, properties->shininess);
            glColor4fv(properties->smoothColor);
         }
      }
   }



   //Draw vertices (maybe with texture coordinates and per vertex normals)
   if(properties->textCoords!=NULL)
   {
      if(properties->invertNormal)
      {
         glTexCoord2fv(properties->textCoords+2);
         Vertex2()->Draw(properties);
         glTexCoord2fv(properties->textCoords+0);
         Vertex1()->Draw(properties);
         glTexCoord2fv(properties->textCoords+4);
         Vertex3()->Draw(properties);
      }
      else
      {
         glTexCoord2fv(properties->textCoords+0);
         Vertex1()->Draw(properties);
         glTexCoord2fv(properties->textCoords+2);
         Vertex2()->Draw(properties);
         glTexCoord2fv(properties->textCoords+4);
         Vertex3()->Draw(properties);
      }
   }
   else
   {
      if(properties->invertNormal)
      {
         Vertex2()->Draw(properties);
         Vertex1()->Draw(properties);
         Vertex3()->Draw(properties);
      }
      else
      {
         Vertex1()->Draw(properties);
         Vertex2()->Draw(properties);
         Vertex3()->Draw(properties);
      }
   }
}

void TriFace::DrawPoints(void) const
{  
    //Draw vertices
    Vertex1()->Draw();
    Vertex2()->Draw();
    Vertex3()->Draw();   
}


void TriFace::CheckDegeneracies(void)
{
   int numDeg=0;
   if(e1->DegenerateMode()==TriEdge::VERTEX)
   {
      numDeg++;
   }
   if(e2->DegenerateMode()==TriEdge::VERTEX)
   {
      numDeg++;
   }
   if(e3->DegenerateMode()==TriEdge::VERTEX)
   {
      numDeg++;
   }
   if(numDeg)
   {
      if(numDeg==3)
      {
         degenerate=VERTEX;
      }
      else
      {
         degenerate=EDGE;
      }
   }
}

bool TriFace::IsFixed(void) const
{
   return Vertex1()->IsFixed() && Vertex2()->IsFixed() && Vertex3()->IsFixed();
}

Real TriFace::MaxCollisionTime(void) const
{
   return max(max(Vertex1()->CollisionTime(), Vertex2()->CollisionTime()), Vertex3()->CollisionTime());
}


